//
// Created by Xiezongxi on 2025/6/7.
//

#ifndef DRV_KALMAN_H
#define DRV_KALMAN_H

typedef struct
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter

// void kalmanFilter_init(void);

float kalmanFilter(KFP *kfp,float input);

float kalmanFilter_Merge(KFP *kfp1,KFP *kfp2,float input1,float input2);

void estimateNoiseParams(KFP *kfp, float *measurements, int n);
#endif //DRV_KALMAN_H
